/***************************************************************
 * The entrance of ROS communication and stanley control
 * @author Huyunhao
 * @date 2020-12-30
 * */
#include <ros/ros.h>
#include "custom_msgs/path_track.h"
#include "stanley_controller.h"

// 定义全局变量来接收和发送ros msg
custom_msgs::path_track msg_rx;     // 接收的msg
custom_msgs::path_track msg_tx;     // 发送的msg

void receiveMsg(const custom_msgs::path_trackConstPtr &msg) {
    ROS_INFO("I receive [x,y,yaw,v]=[%f,%f,%f,%f]", msg->x, msg->y, msg->yaw, msg->v);
    msg_rx = *msg;
}

void runRos(int argc, char **argv) {
    // 初始化ROS节点，并且给节点命名
    ros::init(argc, argv, "n_stanley_controller");
    // 实例化ROS句柄
    ros::NodeHandle n;  // 句柄中封装了ROS的一些常用功能
    // 想要发布的话题
    ros::Publisher publisher = n.advertise<custom_msgs::path_track>("control", 1000);
    // 想要订阅的话题
    ros::Subscriber subscriber = n.subscribe("preScan", 10000, receiveMsg);
    // 发布话题的频率，100HZ
    ros::Rate loop_rate(100);
    //***************************Initialize Stanley Controller parameter*********************
    // 1.初始化车辆状态
    controlNS::State state(0,0,0,0);
    controlNS::StanleyController stanleyController;
    // 2.进入ROS循环
    while (ros::ok()) {
        ros::spinOnce();        // 读取回调函数中接收
        msg_tx.steer_angle = stanleyController.calcDelta(state);
        msg_tx.cross_track_error = stanleyController.cross_track_error_;
        ROS_INFO("I send delta[%f]", msg_tx.steer_angle);
        state.UpdateForPreScan(msg_rx.x, msg_rx.y, msg_rx.yaw, msg_rx.v);   //更新车辆的状态
        publisher.publish(msg_tx);
        loop_rate.sleep();      // 根据前面制定发布话题的频率，进行休眠
    }

}


int main(int argc, char **argv) {
    runRos(argc,argv);
    return 0;
}
